#include "vex.h"
long gt = 0;
void wait(int milsec) { vex::this_thread::sleep_for(milsec); }

int sgn(double x) { return (x > 0) - (x < 0); }

void abs_max_limit(double &x, double max_bnd) {
  max_bnd = abs(max_bnd);
  if (abs(x) > max_bnd)
    x = sgn(x) * max_bnd;
}

void abs_min_limit(double &x, double min_bnd) {
  min_bnd = abs(min_bnd);
  if (abs(x) < min_bnd)
    x = sgn(x) * min_bnd;
}

void value_boundary(double &x, double min, double max) {
  if (x < min)
    x = min;
  if (x > max)
    x = max;
}

void zero_out_blind(double &x, double threshold) {
  threshold = abs(threshold);
  if (abs(x) < threshold)
    x = 0;
}

double battery_vtg_map_range(double bat, double in_min, double in_mabat,
                             double out_min, double out_mabat,
                             double default_out) {
  if (bat != 0)
    return (bat - in_min) * (out_mabat - out_min) / (in_mabat - in_min) +
           out_min;
  else {
    if (default_out == 0)
      return (out_min + out_mabat) / 2;
    else
      return default_out;
  }
}

double get_displacement_pct(double dst_now, double begin_dst, double aim_dst) {
  double agl_all = aim_dst - begin_dst;
  double dst_done = dst_now - begin_dst;
  return dst_done / agl_all * 100; //返回百分比,超过目标则返回值大于100。
}